home *** CD-ROM | disk | FTP | other *** search
/ OpenGL Superbible (2nd Edition) / OpenGL SuperBible e2.iso / tools / GLUT-3.7 / PROGS / EXAMPLES / circlefit.c < prev    next >
Encoding:
C/C++ Source or Header  |  1998-08-12  |  6.5 KB  |  278 lines
  1.  
  2. /* Copyright (c) Mark J. Kilgard, 1997. */
  3.  
  4. /* This program is freely distributable without licensing fees 
  5.    and is provided without guarantee or warrantee expressed or 
  6.    implied. This program is -not- in the public domain. */
  7.  
  8. /* This is a small interactive demo of Dave Eberly's algorithm
  9.    that fits a circle boundary to a set of 2D points. */
  10.  
  11. #include <stdio.h>
  12. #include <stdlib.h>
  13. #include <math.h>
  14. #include <GL/glut.h>
  15.  
  16. /* Some <math.h> files do not define M_PI... */
  17. #ifndef M_PI
  18. #define M_PI 3.14159265358979323846
  19. #endif
  20.  
  21. typedef struct {
  22.   double x, y;
  23. } Point2;
  24.  
  25. /****************************************************************************
  26.    Least squares fit of circle to set of points.
  27.    by Dave Eberly (eberly@cs.unc.edu or eberly@ndl.com)
  28.    ftp://ftp.cs.unc.edu/pub/users/eberly/magic/circfit.c
  29.   ---------------------------------------------------------------------------
  30.    Input:  (x_i,y_i), 1 <= i <= N, where N >= 3 and not all points
  31.            are collinear
  32.    Output:  circle center (a,b) and radius r
  33.   
  34.    Energy function to be minimized is
  35.   
  36.       E(a,b,r) = sum_{i=1}^N (L_i-r)^2
  37.   
  38.    where L_i = |(x_i-a,y_i-b)|, the length of the specified vector.
  39.    Taking partial derivatives and setting equal to zero yield the
  40.    three nonlinear equations
  41.   
  42.    E_r = 0:  r = Average(L_i)
  43.    E_a = 0:  a = Average(x_i) + r * Average(dL_i/da)
  44.    E_b = 0:  b = Average(y_i) + r * Average(dL_i/db)
  45.   
  46.    Replacing r in the last two equations yields
  47.   
  48.      a = Average(x_i) + Average(L_i) * Average(dL_i/da) = F(a,b)
  49.      b = Average(y_i) + Average(L_i) * Average(dL_i/db) = G(a,b)
  50.   
  51.    which can possibly be solved by fixed point iteration as
  52.   
  53.      a_{n+1} = F(a_n,b_n),  b_{n+a} = G(a_n,b_n)
  54.   
  55.    with initial guess a_0 = Average(x_i) and b_0 = Average(y_i).
  56.    Derivative calculations show that
  57.   
  58.      dL_i/da = (a-x_i)/L_i,  dL_i/db = (b-y_i)/L_i.
  59.   
  60.   ---------------------------------------------------------------------------
  61.    WARNING.  I have not analyzed the convergence properties of the fixed
  62.    point iteration scheme.  In a few experiments it seems to converge
  63.    just fine, but I do not guarantee convergence in all cases.
  64.  ****************************************************************************/
  65.  
  66. int
  67. CircleFit(int N, Point2 * P, double *pa, double *pb, double *pr)
  68. {
  69.   /* user-selected parameters */
  70.   const int maxIterations = 256;
  71.   const double tolerance = 1e-06;
  72.  
  73.   double a, b, r;
  74.  
  75.   /* compute the average of the data points */
  76.   int i, j;
  77.   double xAvr = 0.0;
  78.   double yAvr = 0.0;
  79.  
  80.   for (i = 0; i < N; i++) {
  81.     xAvr += P[i].x;
  82.     yAvr += P[i].y;
  83.   }
  84.   xAvr /= N;
  85.   yAvr /= N;
  86.  
  87.   /* initial guess */
  88.   a = xAvr;
  89.   b = yAvr;
  90.  
  91.   for (j = 0; j < maxIterations; j++) {
  92.     /* update the iterates */
  93.     double a0 = a;
  94.     double b0 = b;
  95.  
  96.     /* compute average L, dL/da, dL/db */
  97.     double LAvr = 0.0;
  98.     double LaAvr = 0.0;
  99.     double LbAvr = 0.0;
  100.  
  101.     for (i = 0; i < N; i++) {
  102.       double dx = P[i].x - a;
  103.       double dy = P[i].y - b;
  104.       double L = sqrt(dx * dx + dy * dy);
  105.       if (fabs(L) > tolerance) {
  106.         LAvr += L;
  107.         LaAvr -= dx / L;
  108.         LbAvr -= dy / L;
  109.       }
  110.     }
  111.     LAvr /= N;
  112.     LaAvr /= N;
  113.     LbAvr /= N;
  114.  
  115.     a = xAvr + LAvr * LaAvr;
  116.     b = yAvr + LAvr * LbAvr;
  117.     r = LAvr;
  118.  
  119.     if (fabs(a - a0) <= tolerance && fabs(b - b0) <= tolerance)
  120.       break;
  121.   }
  122.  
  123.   *pa = a;
  124.   *pb = b;
  125.   *pr = r;
  126.  
  127.   return (j < maxIterations ? j : -1);
  128. }
  129.  
  130. enum {
  131.   M_SHOW_CIRCLE, M_CIRCLE_INFO, M_RESET_POINTS, M_QUIT
  132. };
  133.  
  134. #define MAX_POINTS 100
  135.  
  136. int num = 0;
  137. Point2 list[MAX_POINTS];
  138. int circleFitNeedsRecalc = 0;
  139. int showCircle = 1;
  140. int circleInfo = 0;
  141. int windowHeight;
  142. double a, b, r = 0.0;   /* X, Y, and radius of best fit circle. 
  143.                          */
  144.  
  145. void
  146. drawCircle(float x, float y, float r)
  147. {
  148.   double angle;
  149.  
  150.   glPushMatrix();
  151.   glTranslatef(x, y, 0);
  152.   glBegin(GL_TRIANGLE_FAN);
  153.   glVertex2f(0, 0);
  154.   for (angle = 0.0; angle <= 2 * M_PI; angle += M_PI / 24) {
  155.     glVertex2f(r * cos(angle), r * sin(angle));
  156.   }
  157.   glEnd();
  158.   glPopMatrix();
  159. }
  160.  
  161. void
  162. display(void)
  163. {
  164.   int i;
  165.  
  166.   if (circleFitNeedsRecalc) {
  167.     int rc;
  168.  
  169.     rc = CircleFit(num, list, &a, &b, &r);
  170.     if (rc == -1) {
  171.       fprintf(stderr, "circlefit: Problem fitting points to a circle encountered.\n");
  172.     } else {
  173.       if (circleInfo) {
  174.         printf("%g @ (%g,%g)\n", r, a, b);
  175.       }
  176.     }
  177.     circleFitNeedsRecalc = 0;
  178.   }
  179.   glClear(GL_COLOR_BUFFER_BIT);
  180.  
  181.   if (showCircle && r > 0.0) {
  182.     glColor3ub(0xbb, 0xbb, 0xdd);
  183.     drawCircle(a, b, r);
  184.   }
  185.   glColor3ub(0, 100, 0);
  186.   glBegin(GL_POINTS);
  187.   for (i = 0; i < num; i++) {
  188.     glVertex2d(list[i].x, list[i].y);
  189.   }
  190.   glEnd();
  191.   glutSwapBuffers();
  192. }
  193.  
  194. void
  195. reshape(int w, int h)
  196. {
  197.   glViewport(0, 0, w, h);
  198.   glMatrixMode(GL_PROJECTION);
  199.   glLoadIdentity();
  200.   gluOrtho2D(0, w, 0, h);
  201.   glMatrixMode(GL_MODELVIEW);
  202.   windowHeight = h;
  203. }
  204.  
  205. void
  206. addPoint(double x, double y)
  207. {
  208.   if (num + 1 >= MAX_POINTS) {
  209.     fprintf(stderr, "circlefit: limited to only %d points\n", MAX_POINTS);
  210.     return;
  211.   }
  212.   list[num].x = x;
  213.   list[num].y = y;
  214.   num++;
  215.   circleFitNeedsRecalc = 1;
  216.   glutPostRedisplay();
  217. }
  218.  
  219. void
  220. mouse(int button, int state, int x, int y)
  221. {
  222.   if (button == GLUT_LEFT_BUTTON && state == GLUT_DOWN) {
  223.     addPoint(x, windowHeight - y);
  224.   }
  225. }
  226.  
  227. void
  228. menu(int value)
  229. {
  230.   switch (value) {
  231.   case M_SHOW_CIRCLE:
  232.     showCircle = !showCircle;
  233.     break;
  234.   case M_CIRCLE_INFO:
  235.     circleInfo = !circleInfo;
  236.     break;
  237.   case M_RESET_POINTS:
  238.     num = 0;
  239.     r = 0.0;
  240.     break;
  241.   case M_QUIT:
  242.     exit(0);
  243.   }
  244.   glutPostRedisplay();
  245. }
  246.  
  247. int
  248. main(int argc, char **argv)
  249. {
  250.   glutInitWindowSize(400, 400);
  251.   glutInit(&argc, argv);
  252.   glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);
  253.   glutCreateWindow("Least squares fit of circle to set of points");
  254.  
  255.   printf("\n");
  256.   printf("Least squares fit of circle to set of points\n");
  257.   printf("--------------------------------------------\n");
  258.   printf("Click left mouse button to position points.  The\n");
  259.   printf("program then shows the circle whose boundary best\n");
  260.   printf("fits the set of points specified.  Try clicking\n");
  261.   printf("points in a near circle.\n");
  262.   printf("\n");
  263.  
  264.   glClearColor(125.0 / 256.0, 158.0 / 256.0, 192.0 / 256.0, 1.0);
  265.   glPointSize(3.0);
  266.   glutReshapeFunc(reshape);
  267.   glutMouseFunc(mouse);
  268.   glutDisplayFunc(display);
  269.   glutCreateMenu(menu);
  270.   glutAddMenuEntry("Show/hide circle", M_SHOW_CIRCLE);
  271.   glutAddMenuEntry("Toggle info printing", M_CIRCLE_INFO);
  272.   glutAddMenuEntry("Reset points", M_RESET_POINTS);
  273.   glutAddMenuEntry("Quit", M_QUIT);
  274.   glutAttachMenu(GLUT_RIGHT_BUTTON);
  275.   glutMainLoop();
  276.   return 0;             /* ANSI C requires main to return int. */
  277. }
  278.